#include <mathematics/Quaternion.h>

namespace ne
{
    const Quaternion Quaternion::ZERO(0.0f, 0.0f, 0.0f, 0.0f);
    const Quaternion Quaternion::IDENTITY(0.0f, 0.0f, 0.0f, 1.0f);

    Quaternion operator * (const real fScale, const Quaternion &q)
    {
        return Quaternion(fScale * q.x, fScale * q.y, fScale * q.z, fScale * q.w);
    }

    void Quaternion::fromRotationMatrix(const Matrix3 &m)
    {
        // Algorithm in Ken Shoemake's article in 1987 SIGGRAPH course notes
        // article "Quaternion Calculus and Fast Animation".

        real fTrace = m[0][0] + m[1][1] + m[2][2];
        if (fTrace > 0.0f)
        {
            // |w| > 1/2, may as well choose w > 1/2
            real fRoot = std::sqrt(fTrace + 1.0f);  // 2w
            w = 0.5f * fRoot;
            fRoot = 0.5f / fRoot;  // 1/(4w)
            x = (m[1][2] - m[2][1]) * fRoot;
            y = (m[2][0] - m[0][2]) * fRoot;
            z = (m[0][1] - m[1][0]) * fRoot;
        }
        else
        {
            // |w| <= 1/2
            size_t i = 0;
            size_t j = 1;
            size_t k = 2;
            if (m[1][1] > m[0][0])
            {
                i = 1;
                j = 2;
                k = 0;
            }
            if (m[2][2] > m[i][i])
            {
                i = 2;
                j = 0;
                k = 1;
            }

            real fRoot = std::sqrt(m[i][i] - m[j][j] - m[k][k] + 1.0f);
            real* apkQuat[3] = {&x, &y, &z};
            *apkQuat[i] = 0.5f * fRoot;
            fRoot = 0.5f / fRoot;
            w = (m[k][j] - m[j][k]) * fRoot;
            *apkQuat[j] = (m[j][i] + m[i][j]) * fRoot;
            *apkQuat[k] = (m[k][i] + m[i][k]) * fRoot;
        }
    }

    void Quaternion::toRotationMatrix(Matrix3 &m) const
    {
        real fTx  = x + x;
        real fTy  = y + y;
        real fTz  = z + z;
        real fTwx = fTx * w;
        real fTwy = fTy * w;
        real fTwz = fTz * w;
        real fTxx = fTx * x;
        real fTxy = fTy * x;
        real fTxz = fTz * x;
        real fTyy = fTy * y;
        real fTyz = fTz * y;
        real fTzz = fTz * z;

        m[0][0] = 1.0f - (fTyy + fTzz);
        m[0][1] = fTxy + fTwz;
        m[0][2] = fTxz - fTwy;
        m[1][0] = fTxy - fTwz;
        m[1][1] = 1.0f - (fTxx + fTzz);
        m[1][2] = fTyz + fTwx;
        m[2][0] = fTxz + fTwy;
        m[2][1] = fTyz - fTwx;
        m[2][2] = 1.0f - (fTxx + fTyy);
    }

    void Quaternion::fromAxes(const Vector3* const axes)
    {
        Matrix3 m;
        m.fromAxes(axes[0], axes[1], axes[2]);
        fromRotationMatrix(m);
    }

    void Quaternion::fromAxes(const Vector3 &vX, const Vector3 &vY, const Vector3 &vZ)
    {
        Matrix3 m;
        m.fromAxes(vX, vY, vZ);
        fromRotationMatrix(m);
    }

    void Quaternion::toAxes(Vector3 &vX, Vector3 &vY, Vector3 &vZ) const
    {
        Matrix3 m;
        toRotationMatrix(m);
        vX = m.getRow(0);
        vY = m.getRow(1);
        vZ = m.getRow(2);
    }

    void Quaternion::fromAxisAngle(const Vector3 &axis, const real a)
    {
        // assert:  axis[] is unit length
        //
        // The quaternion representing the rotation is
        //   q = cos(A/2) + sin(A/2)*(x*i + y*j + z*k)

        real halfA = 0.5f * a;
        real fSin = std::sin(halfA);
        x = fSin * axis.x;
        y = fSin * axis.y;
        z = fSin * axis.z;
        w = std::cos(halfA);
    }

    bool Quaternion::toAxisAngle(Vector3 &axis, real &a) const
    {
        // The quaternion representing the rotation is
        //   q = cos(A/2) + sin(A/2)*(x*i + y*j + z*k)

        real fSqrLength = x * x + y * y + z * z;
        if (fSqrLength > 0.0f)
        {
            a = 2.0f * std::acos(w);
            real invLen = 1.0f / fSqrLength;
            axis.x = x * invLen;
            axis.y = y * invLen;
            axis.z = z * invLen;
            return true;
        }

        // angle is 0 (mod 2*pi), so any axis will do
        a = 0.0f;
        axis.x = 0.0f;
        axis.y = 1.0f;
        axis.z = 0.0f;
        return false;
    }

    real Quaternion::getAngle() const
    {
        real fSqrLength = x * x + y * y + z * z;
        if (fSqrLength > 0.0f)
        {
            return 2.0f * std::acos(w);
        }
        return 0.0f;
    }

    Vector3 Quaternion::getAxis() const
    {
        Vector3 axis;
        real fSqrLength = x * x + y * y + z * z;
        if (fSqrLength > 0.0f)
        {
            real invLen = 1.0f / fSqrLength;
            axis.x = x * invLen;
            axis.y = y * invLen;
            axis.z = z * invLen;
        }
        else
        {
            // angle is 0 (mod 2*pi), so any axis will do
            axis.x = 0.0f;
            axis.y = 1.0f;
            axis.z = 0.0f;
        }
        return axis;
    }

    void Quaternion::fromEulerAngles(const real yaw, const real pitch, const real roll)
    {
        real fCosX = std::cos(0.5f * pitch);
        real fSinX = std::sin(0.5f * pitch);
        real fCosY = std::cos(0.5f * yaw);
        real fSinY = std::sin(0.5f * yaw);
        real fCosZ = std::cos(0.5f * roll);
        real fSinZ = std::sin(0.5f * roll);
        x = fCosY * fSinX * fCosZ + fSinY * fCosX * fSinZ;
        y = fSinY * fCosX * fCosZ - fCosY * fSinX * fSinZ;
        z = fCosY * fCosX * fSinZ - fSinY * fSinX * fCosZ;
        w = fCosY * fCosX * fCosZ + fSinY * fSinX * fSinZ;
    }

    void Quaternion::toEulerAngles(real &yaw, real &pitch, real &roll) const
    {
        real fSinX = -2.0f * (y * z - w * x);
        if (std::abs(fSinX) > 1.0f)
        {
            pitch = 1.570796f * fSinX;
            yaw = std::atan2(real(-x * z + w * y), real(0.5f - y * y - z * z));
            roll = 0.0f;
        }
        else
        {
            pitch = std::asin(fSinX);
            yaw = std::atan2(real(x * z + w * y), real(0.5f - x * x - y * y));
            roll = std::atan2(real(x * y + w * z), real(0.5f - x * x - z * z));
        }
    }

    void Quaternion::fromAxisXAngle(const real a)
    {
        real halfA = 0.5f * a;
        real fSin = std::sin(halfA);
        x = fSin;
        y = 0.0f;
        z = 0.0f;
        w = std::cos(halfA);
    }

    void Quaternion::fromAxisYAngle(const real a)
    {
        real halfA = 0.5f * a;
        real fSin = std::sin(halfA);
        x = 0.0f;
        y = fSin;
        z = 0.0f;
        w = std::cos(halfA);
    }

    void Quaternion::fromAxisZAngle(const real a)
    {
        real halfA = 0.5f * a;
        real fSin = std::sin(halfA);
        x = 0.0f;
        y = fSin;
        z = 0.0f;
        w = std::cos(halfA);
    }

    Quaternion Quaternion::MakeRotation(const Vector3 &vs, const Vector3 &vd)
    {
        Quaternion q;
        Vector3 v0 = vs;
        real len0 = v0.normalize();
        Vector3 v1 = vd;
        real len1 = v1.normalize();
        real d = v0.dotProduct(v1);
        if ((len0 == 0.0f) || (len1 == 0.0f) || (d >= 1.0f))
        {
            return IDENTITY;
        }

        if (d <= -1.0f)
        {
            Vector3 axis = v0.crossProduct(Vector3::UNIT_X);
            if (axis.squaredLength() == 0.0f)
            {
                axis = v0.crossProduct(Vector3::UNIT_Y);
                if (axis.squaredLength() == 0.0f)
                {
                    axis = v0.crossProduct(Vector3::UNIT_Z);
                }
            }
            q.fromAxisAngle(axis, Math::PI);
        }
        else
        {
            Vector3 axis = v0.crossProduct(v1);
            real fCos = std::sqrt(0.5f * (d + 1.0f));
            real fSin = std::sqrt(0.5f * (1.0f - d));
            q.x = fSin * axis.x;
            q.y = fSin * axis.y;
            q.z = fSin * axis.z;
            q.w = fCos;
            q.normalize();
        }
        return q;
    }

    Quaternion Quaternion::Lerp(const Quaternion &pQ, const Quaternion &qQ,
                                const real u, const bool shortestPath)
    {
        Quaternion resQ;
        real fCos = pQ.dotProduct(qQ);
		if (shortestPath && fCos < 0.0f)
		{
			resQ = pQ + ((-qQ) - pQ) * u;
		}
		else
		{
			resQ = pQ + (qQ - pQ) * u;
		}
        resQ.normalize();
        return resQ;
    }

    Quaternion Quaternion::Slerp(const Quaternion &pQ, const Quaternion &qQ,
                                 const real u, const bool shortestPath)
    {
        real fCos = pQ.dotProduct(qQ);
        Quaternion tempQ;

        // Do we need to invert rotation?
        if (shortestPath && fCos < 0.0f)
        {
            fCos = -fCos;
            tempQ = -qQ;
        }
        else
        {
            tempQ = qQ;
        }

        if (std::abs(fCos) < 1.0f)
        {
            // Standard case (slerp)
            real fSin = std::sqrt(1.0f - fCos * fCos);
            real fAngle = std::atan2(fSin, fCos);
            real fInvSin = 1.0f / fSin;
            real fCoeff0 = std::sin((1.0f - u) * fAngle) * fInvSin;
            real fCoeff1 = std::sin(u * fAngle) * fInvSin;
            return pQ * fCoeff0 + tempQ * fCoeff1;
        }
        else
        {
            // There are two situations:
            // 1. "pQ" and "qQ" are very close (fCos ~= +1), so we can do a linear
            //    interpolation safely.
            // 2. "pQ" and "qQ" are almost inverse of each other (fCos ~= -1), there
            //    are an infinite number of possibilities interpolation. but we haven't
            //    have method to fix this case, so just use linear interpolation here.
            Quaternion resQ = pQ + (tempQ - pQ) * u;
            // taking the complement requires renormalisation
            resQ.normalize();
            return resQ;
        }
    }

    Quaternion Quaternion::Squad(const Quaternion &pQ, const Quaternion &aQ,
                                 const Quaternion &bQ, const Quaternion &qQ,
                                 const real u, const bool shortestPath)
    {
        real fSlerpU = 2.0f * u * (1.0f - u);
        Quaternion kSlerpP = Slerp(pQ, qQ, u, shortestPath);
        Quaternion kSlerpQ = Slerp(aQ, bQ, u);
        return Slerp(kSlerpP, kSlerpQ, fSlerpU);
    }

    std::ostream& operator << (std::ostream &o, const Quaternion &q)
    {
        o << "Quaternion(x=" << q.x <<
                      ", y=" << q.y <<
                      ", z=" << q.z <<
                      ", w=" << q.w << ")";
        return o;
    }
}
